package touchphysics.physics.collision;


import java.util.Arrays;
import java.util.Comparator;
import java.util.LinkedList;

import touchphysics.math.Vector2f;

/**
 * 
 * @author Valerij Wittenbeck
 *
 */
public class IntersectionGatherer {
	
	/**  */
	public static float MIN_PAIR_DIST = 0.5f;
	/**  */
	public static int MAX_INTERSECTIONS = 50;
	/**  */
	private SortableIntersection[] intersections;
	/**  */
	private int noIntersections = 0;
	
	/**  */
	private Vector2f[] vertsA;
	/**  */
	private Vector2f[] vertsB;

	/**
	 * 
	 * @param vertsA 
	 * @param vertsB 
	 */
	public IntersectionGatherer(Vector2f[] vertsA, Vector2f[] vertsB) {
		this.intersections = new SortableIntersection[MAX_INTERSECTIONS];
		this.noIntersections = 0;
		this.vertsA = vertsA;
		this.vertsB = vertsB;
	}
	
	/**
	 * 
	 * @param a 
	 * @param b 
	 */
	public void intersect(int a, int b) {
		if ( noIntersections >= MAX_INTERSECTIONS )
			return;
		
		Vector2f startA = vertsA[a];
		Vector2f endA = vertsA[(a+1) % vertsA.length ];
		Vector2f startB = vertsB[b];
		Vector2f endB = vertsB[(b+1) % vertsB.length ];
		//TODO: reuse mathutil.intersect
		float d = (endB.y - startB.y) * (endA.x - startA.x) - (endB.x - startB.x) * (endA.y - startA.y);
		
		if ( d == 0 ) // parallel lines
			return;
		
		float uA = (endB.x - startB.x) * (startA.y - startB.y) - (endB.y - startB.y) * (startA.x - startB.x);
		uA /= d;
		float uB = (endA.x - startA.x) * (startA.y - startB.y) - (endA.y - startA.y) * (startA.x - startB.x);
		uB /= d;
		
		if ( uA < 0 || uA > 1 || uB < 0 || uB > 1 ) 
			return; // intersection point isn't between the start and endpoints
		
		Vector2f position = new Vector2f(
				startA.x + uA * (endA.x - startA.x),
				startA.y + uA * (endA.y - startA.y));
		
		Vector2f dist = new Vector2f(position);
		dist.sub(startA);
		float distFromVertA = dist.lengthSquared();
		dist = new Vector2f(position);
		dist.sub(startB);
		float distFromVertB = dist.lengthSquared();
		
		// z axis of 3d cross product
		float sA = (startA.x - startB.x) * (endB.y - startB.y) - (endB.x - startB.x) * (startA.y - startB.y);
		
		if ( sA > 0 ) {
			intersections[noIntersections] =
				new SortableIntersection(a, b, position, true, distFromVertA, distFromVertB);
		} else {
			intersections[noIntersections] =
				new SortableIntersection(a, b, position, false, distFromVertA, distFromVertB);
		}
		
		noIntersections++;
	}
	
	/**
	 * 
	 * @return 
	 */
	public Intersection[] getIntersections() {
		Intersection[] out = new Intersection[noIntersections];
		
		for ( int i = 0; i < noIntersections; i++ )
			out[i] = intersections[i];
		
		Arrays.sort(out, new IntersectionComparator());
		
		return out;
	}

	/**
	 *  
	 * @return 
	 */
	public Intersection[][] getIntersectionPairs() {
		if ( noIntersections < 2 )
			return new Intersection[0][2];
		
		Arrays.sort(intersections, 0, noIntersections, new IntersectionComparator());
		
		Integer[] pointers = new Integer[noIntersections];
		for ( int i = 0; i < noIntersections; i++ )
			pointers[i] = new Integer(i);
		Arrays.sort(pointers, new PointerTableComparator());
		
		int referenceVertB = getReferencePointer(pointers);
		filterIntersections(referenceVertB, pointers);
			
		int first = intersections[0].isIngoing ? 0 : 1;
		
		LinkedList outIntersections = new LinkedList();
		for ( int i = first; i < noIntersections + first; ) {
			SortableIntersection in = intersections[i % noIntersections];
			SortableIntersection out = intersections[(i+1) % noIntersections];
			
			if ( in == null ) {
				i += 1;
				continue;
			}
			
			if ( out != null && in.isIngoing && !out.isIngoing ) {
				if ( !in.position.equalsDelta(out.position, MIN_PAIR_DIST) ) {
					Intersection[] pair = {in, out}; 
					outIntersections.add(pair);
					i += 2;
					continue;
				}
			}
			
			Intersection[] inArr = {in};
			outIntersections.add(inArr);
			i += 1;
		}

 		return (Intersection[][]) outIntersections.toArray(new Intersection[outIntersections.size()][]);
	}
	
	/**
	 * Gets an outgoing intersection that seems appropriate to start the
	 * intersection filter with. This implements the heuristic described in the
	 * class documentation.
	 * 
	 * @param pointers An array of pointers which are the indices of the
	 * intersections array. The list should be sorted with the order defined by
	 * {@link PointerTableComparator}.
	 * @return The reference pointer which is an outgoing intersection. 
	 */
	private int getReferencePointer(Integer[] pointers) {
		int first = intersections[pointers[0].intValue()].isIngoing ? 0 : 1;
		int maxInOutDist = 0;
		int maxInIndex = first + 1 % noIntersections;
		int lastInEdgeB = -1;	
		for ( int i = first; i < noIntersections + first; i++ ) {
			int k = pointers[i % noIntersections].intValue();
			SortableIntersection intersection = intersections[k];
			
			if ( intersection.isIngoing ) {
				lastInEdgeB = intersection.edgeB;
			} else if ( lastInEdgeB >= 0 ) {
				int inOutDist = (intersection.edgeB - lastInEdgeB + vertsB.length) % vertsB.length;
				
				// did we find a new max dist?
				if ( inOutDist > maxInOutDist ) {
					maxInOutDist = inOutDist;
					maxInIndex = i % noIntersections;
				}
				lastInEdgeB = -1;
			} 
		}
		
		return maxInIndex;
	}
	
	/**
	 * 
	 * @param referencePointer 
	 * @param pointers 
	 */
	private void filterIntersections(int referencePointer, Integer[] pointers) {
		if ( referencePointer >= noIntersections && referencePointer < 0 )
			throw new RuntimeException("The reference vertex cannot be correct since B does not have that many vertices.");
			
		int topOut = -2; 
		for ( int i = referencePointer; i < noIntersections + referencePointer; i++ ) {
			int j = i % noIntersections;
			int k = pointers[j].intValue();
			SortableIntersection intersection = intersections[k];
			
			if ( intersection.isIngoing ) {
				if ( (topOut - 1 + noIntersections) % noIntersections == k ) { 
					topOut = -2; 
				} else {
					intersections[k] = null; 
				}
			} else {
				if ( topOut < 0 ) {
					topOut = k; 
				} else {
					intersections[k] = null; 
				}
			}
		}
		
		int noRemoved = 0;
		for ( int i = 0; i < noIntersections; i++ ) {
			if ( intersections[i] == null ) {
				noRemoved++;
			} else {
				intersections[i-noRemoved] = intersections[i];
			}
		}
		noIntersections -= noRemoved;
	}

	/** */
	class SortableIntersection extends Intersection {		
		/**  */
		public float distFromVertA;
		/**  */
		public float distFromVertB;

		/**
		 * 
		 * @param edgeA 
		 * @param edgeB 
		 * @param position 
		 * @param isIngoing 
		 * @param distFromVertA 
		 * @param distFromVertB 
		 */
		public SortableIntersection(int edgeA, int edgeB, Vector2f position, boolean isIngoing, float distFromVertA, float distFromVertB) {
			super(edgeA, edgeB, position, isIngoing);
			this.distFromVertA = distFromVertA;
			this.distFromVertB = distFromVertB;
		}
	}
	
	/** */
	class IntersectionComparator implements Comparator {

		/** 
		 * @see Comparator#compare(Object, Object)
		 */
		public int compare(Object first, Object second) {
			SortableIntersection one = (SortableIntersection) first;
			SortableIntersection other = (SortableIntersection) second;
					
			if ( one.edgeA < other.edgeA ) {
				return -1; 
			} else if ( one.edgeA == other.edgeA ) {
				if ( one.distFromVertA < other.distFromVertA ) 
					return -1;
				else if ( one.distFromVertA == other.distFromVertA && one.isIngoing )
					return -1;
			}
				
			return 1;
		}
	}
	
	/** */
	class PointerTableComparator implements Comparator {

		/** 
		 * @see Comparator#compare(Object, Object)
		 */
		public int compare(Object first, Object second) {
			SortableIntersection one = intersections[((Integer) first).intValue()];
			SortableIntersection other = intersections[((Integer) second).intValue()];
			
			if ( one.edgeB < other.edgeB ) {
				return -1; 
			} else if ( one.edgeB == other.edgeB ) {
				if ( one.distFromVertB < other.distFromVertB )
					return -1;
				else if ( one.distFromVertB == other.distFromVertB && !one.isIngoing )
					return -1;
			}
				
			return 1;
		}
	}
	
	

}
